#include "smopos.h"

void SMO_Run(SMOPOS *v)
{
	/*	Sliding mode current observer	*/	
	v->EstIalpha = v->Fsmopos * v->EstIalpha + v->Gsmopos*(v->Valpha-v->Ealpha-v->Zalpha);
	v->EstIbeta = v->Fsmopos * v->EstIbeta + v->Gsmopos*(v->Vbeta-v->Ebeta-v->Zbeta);
	
	/*	Current errors	*/																		
    v->IalphaError = v->EstIalpha - v->Ialpha;
    v->IbetaError  = v->EstIbeta  - v->Ibeta;
    																								
	/*  Sliding control calculator	*/																	
	/* v.Zalpha=v.IalphaError*v.Kslide/v.E0) where E0=0.5 here*/	
	v->IalphaError = (v->IalphaError > v->E0)?(v->E0):(v->IalphaError);
	v->IalphaError = (v->IalphaError <-v->E0)?(-v->E0):(v->IalphaError);
	
	v->IbetaError = (v->IbetaError > v->E0)?(v->E0):(v->IbetaError);
	v->IbetaError = (v->IbetaError <-v->E0)?(-v->E0):(v->IbetaError);
	
	v->Zalpha = v->IalphaError*(v->Kslide<<1);
	v->Zbeta = v->IbetaError*(v->Kslide<<1);
																										
	/*	Sliding control filter -> back EMF calculator	*/												
    v->Ealpha = v->Ealpha + (v->Kslf*(v->Zalpha-v->Ealpha));										
    v->Ebeta  = v->Ebeta  + (v->Kslf*(v->Zbeta -v->Ebeta));											
																										
	/*	Rotor angle calculator -> Theta = atan(-Ealpha,Ebeta)	*/										
//	v.Theta = _IQatan2PU(-v.Ealpha,v.Ebeta); 


	/* PLL Rotor angle calculator */
	v->err = -v->Ealpha*v->cosine-v->Ebeta*v->sine;
	v->up = v->kp * v->err;
	v->ui += v->ki * v->err;
	v->omega = v->up + v->ui;
	
	v->Theta += v->omega;
}
